// Field & Service Robotics
// Team 5
// Mars Manipulator
// code for the microcontroller to control linear actuator

#include <Servo.h>

// msg protocol:
// v###;
// v specifies the linear actuator
// ### specifies the desired position, from 0 (fully retracted) to 255 (fully extended)
// ; specifies the end of the message
// ex. v127; would tell the actuator to go to the halfway point

#define MSG_LENGTH 5 // length of expected msg
#define MSG_TYPE_POS 0 // the servo type specified at the 0 position of msg
#define MSG_TYPE_VERT 'v'
#define MSG_END ';'

int vertPin = 10; // linear actuator pin, PWN
int pVert = 0; // position of gripper servo
boolean msgEnd = false; // boolean to control while loop, signifying end of received serial msg
int msgPtr = 0; // index of incoming character array from serial comm
char msg[MSG_LENGTH] = {'0','0','0','0','0'}; // incoming serial string from computer, initialized to 0's


void setup()
{  
  Serial.begin(9600); // start serial port at 9600 bps:
  establishContact(); // send a byte to establish contact until receiver responds                                      
  
  pinMode(vertPin,OUTPUT); // set linear actuator pin to output mode to ctrl actuator
  analogWrite(vertPin,0); // fully retract linear actuator 0 = retract to 255 = extend
  delay(1000);
}


void loop() 
{ 
  
  // receive input from computer
  // if we get a valid byte, read input
  // until we reach the terminating character (MSG_END), and serial bytes are available, loop to 
  // put the entire received msg into the msg character array
  while ((Serial.available() > 0) && !msgEnd && msgPtr < MSG_LENGTH) 
  {
    Serial.println("Reading msg");
    
    msg[msgPtr] = Serial.read(); // get incoming byte (a single character)

    if (msg[msgPtr] == MSG_END)
    {
      msgEnd = true;      
    } else {
      msgEnd = false;      
      msgPtr++;
    }
  }    
  
  // if we have reached the end of the msg...  
  if (msgEnd == true)
  {
    
    // check to see correct msg has been received (sends the entire msg back to serial)
    Serial.println("Message received: ");
    Serial.print(msg);
    Serial.println();
 
    // now, decompose the msg into type and position
    // for the moment, there is only one type (wrist type is commented out)
    Serial.println("Parsing msg");
    
    char type = msg[MSG_TYPE_POS]; // determine what type of msg it is/for which servo
    //Serial.print(type);
    //Serial.println();
    
    if (type == MSG_TYPE_VERT)
    {
      // use atoi to convert the ### characters into an int
      pVert = atoi(&msg[1]);
      
      if (pVert < 0 || pVert > 255) {
        Serial.print("ERROR: pVert outside linear actuator range!");  
      }
      // just to check if the correct command was received/converted
      Serial.print("Writing to linear actuator: ");
      Serial.print(pVert);
      Serial.println();
      
      // write position to servo
      analogWrite(vertPin,pVert);
    
    } 
   /* else if (type == MSG_TYPE_WRIST)
    {
      int pWrist = atoi(&msg[1]);
      Serial.print(pWrist);
      Serial.println();
      pWrist = map(pWrist,0,9,1,180);
      
      Serial.print("Writing to wrist servo: ");
      Serial.print(pWrist);
      Serial.println();
      
      gripServo.write(pWrist);
      
    } */
    else {
      Serial.print("ERROR: Unknown msg type - ");
      Serial.print("Message: ");
      Serial.println(msg);
    }    
    
    // otherwise msg was not recognized
  } else if (msg[0] == 0 && msg[1] == 0 && msg[2] == 0 && msg[3] == 0 && msg[4] == 0) {
    Serial.println("Msg empty - do nothing.");
  } else {
    Serial.print("ERROR: Unkown msg - ");
    Serial.print("Message: ");
    Serial.println(msg);
  }
  
  
  // reset variables for loop
  msgEnd = false;
  msgPtr = 0;
  for (int i = 0; i < MSG_LENGTH; i++) 
  {
    msg[i] = 0;
  }
    
  delay(1000); // wait
} 

// send a byte to establish contact until receiver responds                                      
void establishContact() {
  while (Serial.available() <= 0) {
    Serial.print('A', BYTE);   // send a capital A
    delay(300);
  }
}

